use core::marker::PhantomData;
use embedded_hal::i2c::{
    ErrorKind, ErrorType, I2c, NoAcknowledgeSource, Operation, SevenBitAddress,
};
use core::ptr::addr_of_mut;

pub trait SoftI2c {
    fn read_sda() -> bool;
    fn set_scl();
    fn set_sda();
    fn reset_scl();
    fn reset_sda();
    fn delay() {
        let mut value = 0;
        while value < 1000 {
            let n = value + 1;
            unsafe {
                addr_of_mut!(value).write_volatile(n);
            }
        }
    }
}
#[derive(Debug)]
pub struct SI2c<T> {
    _m: PhantomData<T>,
}

impl <T: SoftI2c> SI2c<T> {
    pub const fn const_new() -> Self {
        Self { _m: PhantomData }
    }
}

impl<T: SoftI2c> SI2c<T> {
    fn start() {
        T::set_scl();
        T::set_sda();
        T::delay();
        T::reset_sda();
        T::delay();
        T::reset_scl();
        T::delay();
    }

    fn stop() {
        T::reset_sda();
        T::set_scl();
        T::delay();
        T::set_sda();
        T::delay();
    }

    fn write_byte(value: u8) {
        let mut value = value;
        for _ in 0..8 {
            if (value & 0x80) != 0x00 {
                T::set_sda();
            } else {
                T::reset_sda();
            }
            value = value << 1;
            T::delay();
            T::set_scl();
            T::delay();
            T::reset_scl();
            T::delay();
        }
    }

    fn recv_byte() -> u8 {
        let mut value = 0;
        T::set_sda();
        for _ in 0..8 {
            T::set_scl();
            T::delay();
            value = value << 1;
            if T::read_sda() {
                value |= 0x01;
            }
            T::delay();
            T::reset_scl();
            T::delay();
        }
        value
    }

    fn write_ack() {
        T::reset_sda();
        T::delay();
        T::set_scl();
        T::delay();
        T::reset_scl();
        T::delay();
    }

    fn write_no_ack() {
        T::set_sda();
        T::delay();
        T::set_scl();
        T::delay();
        T::reset_scl();
        T::delay();
    }

    fn wait_ack() -> bool {
        T::set_sda();
        T::set_scl();
        T::delay();
        let flag = !T::read_sda();
        T::delay();
        T::reset_scl();
        T::delay();
        flag
    }
}

impl<T: SoftI2c> ErrorType for SI2c<T> {
    type Error = ErrorKind;
}

impl<T: SoftI2c> I2c<SevenBitAddress> for SI2c<T> {
    fn read(&mut self, address: u8, read: &mut [u8]) -> Result<(), Self::Error> {
        Self::start();
        Self::write_byte((address << 1) | 0x01);
        if !Self::wait_ack() {
            Self::stop();
            return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
        }
        let len = read.len();
        for (index, value) in read.iter_mut().enumerate() {
            *value = Self::recv_byte();
            if index == (len - 1) {
                Self::write_no_ack();
            } else {
                Self::write_ack();
            }
        }
        Self::stop();
        Ok(())
    }

    fn transaction(
        &mut self,
        address: u8,
        operations: &mut [Operation<'_>],
    ) -> Result<(), Self::Error> {
        for oper in operations.iter_mut() {
            match oper {
                Operation::Read(buf) => {
                    Self::start();
                    Self::write_byte((address << 1) | 0x01);
                    if !Self::wait_ack() {
                        Self::stop();
                        return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
                    }
                    let len = buf.len();
                    for (index, value) in buf.iter_mut().enumerate() {
                        *value = Self::recv_byte();
                        if index == (len - 1) {
                            Self::write_no_ack();
                        } else {
                            Self::write_ack();
                        }
                    }
                }
                Operation::Write(buf) => {
                    Self::start();
                    Self::write_byte(address << 1);
                    if !Self::wait_ack() {
                        Self::stop();
                        return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
                    }
                    for value in buf.iter() {
                        Self::write_byte(*value);
                        if !Self::wait_ack() {
                            Self::stop();
                            return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Data));
                        }
                    }
                }
            }
        }
        Self::stop();
        Ok(())
    }

    fn write(&mut self, address: u8, write: &[u8]) -> Result<(), Self::Error> {
        Self::start();
        Self::write_byte(address << 1);
        if !Self::wait_ack() {
            Self::stop();
            return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
        }
        for value in write {
            Self::write_byte(*value);
            if !Self::wait_ack() {
                Self::stop();
                return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Data));
            }
        }
        Self::stop();
        Ok(())
    }

    fn write_read(
        &mut self,
        address: u8,
        write: &[u8],
        read: &mut [u8],
    ) -> Result<(), Self::Error> {
        Self::start();
        Self::write_byte(address << 1);
        if !Self::wait_ack() {
            Self::stop();
            return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
        }
        for value in write {
            Self::write_byte(*value);
            if !Self::wait_ack() {
                Self::stop();
                return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Data));
            }
        }
        Self::start();
        Self::write_byte((address << 1) | 0x01);
        if !Self::wait_ack() {
            Self::stop();
            return Err(ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address));
        }
        let len = read.len();
        for (index, value) in read.iter_mut().enumerate() {
            *value = Self::recv_byte();
            if index == (len - 1) {
                Self::write_no_ack();
            } else {
                Self::write_ack();
            }
        }
        Self::stop();
        Ok(())
    }
}
